/**
 * @file trace.c
 * @author sgfz
 * @brief PD权值巡线 桥上巡线 PD加减速 三角形加减速
 * @version 0.1
 * @date 2024-09-01
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "trace.h"
#include "bsp_adc.h"
#include "bsp_motor.h"
#include "bsp_SysTick.h"
#include "bsp_usart.h"
#include "reset.h"
#include "posture.h"
#include "bsp_usart.h"

int Gray_sum;
int speed;
int lastError;
int motorSpeed;
int lastm1Speed; // 上次速度
int lastm2Speed;
int mSpeed_right;
int mSpeed_left;
uint8_t cnt_whiteline;
uint8_t cnt_whitelineL;
uint8_t cnt_whitelineR;
double sum = 0.0;
float error;
float KP;
float KD;

/**
 * @brief PD权值法巡线+干扰线条件+限幅
 */
void Trace(void)
{

    // printf("err=%.2f, R=%d, L=%d\n", error, mSpeed_right, mSpeed_left);
    // 绿地都小于120
    // 白线都大于250
    if (Huidu_va(1) > white && Huidu_va(2) > white && Huidu_va(3) > white && Huidu_va(4) > white &&
        Huidu_va(5) > white && Huidu_va(6) > white && Huidu_va(7) > white && Huidu_va(8) > white &&
        Huidu_va(9) > white && Huidu_va(10) > white) {
        error = 0;
    } else if (Huidu_va(1) < green && Huidu_va(2) < green && Huidu_va(3) < green && Huidu_va(4) < green &&
               Huidu_va(5) < green && Huidu_va(6) < green && Huidu_va(7) < green && Huidu_va(8) < green &&
               Huidu_va(9) < green && Huidu_va(10) < green) {
        error = 0;
    } else if (Huidu_va(5) > white && Huidu_va(6) > white) {
        error = 0;
    } else if ((Huidu_va(5) > white || Huidu_va(6) > white) && (Huidu_va(0) > white || Huidu_va(1) > white)) {
        error = 0;
    } else if ((Huidu_va(5) > white || Huidu_va(6) > white) && (Huidu_va(11) > white || Huidu_va(10) > white)) {
        error = 0;
    } else if (cnt_whiteline >= 3) {
        error = 0;
    } else {
        Gray_sum = Huidu_va(0) * (3) + Huidu_va(1) * (5) + Huidu_va(2) * (4) + Huidu_va(3) * (3) + Huidu_va(4) * (2) + Huidu_va(5) * (1) +
                   Huidu_va(11) * (-3) + Huidu_va(10) * (-5) + Huidu_va(9) * (-4) + Huidu_va(8) * (-3) + Huidu_va(7) * (-2) + Huidu_va(6) * (-1);
        error = Gray_sum * 1.0 * (3000.0 / sum * 1.0); // 3600
    }

    motorSpeed = KP * error + KD * (error - lastError);
    lastError  = error;
    // process_tx_buffer();
    mSpeed_right = speed - motorSpeed; // 右轮速度
    mSpeed_left  = speed + motorSpeed; // 左轮速度
    lastm1Speed  = mSpeed_right;
    lastm2Speed  = mSpeed_left;
    // 限幅
    if (speed < 160) {
        if (mSpeed_right < 0) {
            mSpeed_right = 0;
        } else if (mSpeed_right > (1.5 * speed)) {
            mSpeed_right = 1.5 * speed;
        }

        if (mSpeed_left < 0) {
            mSpeed_left = 0;
        } else if (mSpeed_left > (1.5 * speed)) {
            mSpeed_left = 1.5 * speed;
        }
    }
    if (speed >= 160) {
        if (mSpeed_right < 0) {
            mSpeed_right = 0;
        } else if (mSpeed_right > (1.35 * speed)) {
            mSpeed_right = 1.35 * speed;
        }

        if (mSpeed_left < 0) {
            mSpeed_left = 0;
        } else if (mSpeed_left > (1.35 * speed)) {
            mSpeed_left = 1.35 * speed;
        }
    }

    set_pwm(1, mSpeed_right, speed);
    set_pwm(2, mSpeed_left, speed);
}
/**
 * @brief 高速巡线, 速度可调范围90 - 220
 * @param N 设定速度
 */
void high_run(int N)
{
    get_huidu_va();
    speed = N;
    // if (speed < 100) {
    //     KP = 0.008;
    //     KD = 0.0200;
    // } else if (speed >= 100 && speed < 140) {
    //     KP = 0.002;
    //     KD = 0.0236;
    // } else if (speed >= 140 && speed < 160) {
    //     KP = 0.0022;
    //     KD = 0.0264;
    // } else if (speed >= 160 && speed < 180) {
    //     KP = 0.00205;
    //     KD = 0.0266;
    // } else if (speed >= 180 && speed < 200) {
    //     KP = 0.00205;//0.0025
    //     KD = 0.0267;//0.0264
    // } else if (speed >= 200 && speed <= 210) {
    //     KP = 0.0019;
    //     KD = 0.0300;
    // } else if (speed > 210 && speed <= 220) {
    //     KP = 0.0020;//0.0019
    //     KD = 0.0305;//0.0310
    // }
    if (speed < 100) {
        KP = 0.008;
        KD = 0.0200;
    } else if (speed >= 100 && speed < 140) {
        KP = 0.002;
        KD = 0.0236;
    } else if (speed >= 140 && speed < 160) {
        KP = 0.0022;
        KD = 0.0264;
    } else if (speed >= 160 && speed < 180) {
        KP = 0.00202; // 0.00203
        KD = 0.02695; // 0.0269
    } else if (speed >= 180 && speed < 200) {
        KP = 0.00203; // 0.00205
        KD = 0.02696; // 0.02696
    } else if (speed >= 200 && speed < 210) {
        KP = 0.0019;
        KD = 0.03155; // 0.03006
    } else if (speed >= 210 && speed <= 220) {
        KP = 0.0019;  // 0.0019
        KD = 0.03155; // 0.0310
    }
    Trace();
}
/**
 * @brief 低速巡线, 速度可调范围30 - 100
 * @param N 设定速度
 */
void slow_run(int N)
{
    get_huidu_va();
    speed = N;
    // if (speed < 50) {
    //     KP = 0.04; // 0.015
    //     KD = 0.09; // 0.05
    // } else if (speed >= 50 && speed < 70) {
    //     KP = 0.007; // 0.015
    //     KD = 0.05;   // 0.01
    // } else if (speed >= 70 && speed <= 85) {
    //     KP = 0.007; // 0.015
    //     KD = 0.04;  // 0.02
    // } else {
    //     KP = 0.011; // 0.015
    //     KD = 0.09;  // 0.03
    // }
    if (speed < 50) {
        KP = 0.04; // 0.015
        KD = 0.09; // 0.05
    } else if (speed >= 50 && speed < 70) {
        KP = 0.007; // 0.015
        KD = 0.05;  // 0.01
    } else if (speed >= 70 && speed <= 85) {
        KP = 0.007; // 0.015
        KD = 0.04;  // 0.02
    } else {
        KP = 0.011; // 0.015
        KD = 0.09;  // 0.03
    }
    Trace();
}

void speed_up(int start, int end)
{
    KP = 0.001;
    KD = 0.025;
    // for (; start < end; start++) {
    //     if (speed < 100) {
    //         KP = 0.013;
    //         KD = 0.07;
    //     } else {
    //         KP = 0.010;
    //         KD = 0.10;
    //     }
    //     speed = start;
    for (; start < end; start++) {
        if (speed < 100) {
            KP = 0.013;
            KD = 0.07;
        } else {
            KP = 0.009;
            KD = 0.10;
        }
        speed = start;
        get_huidu_va();
        Trace();
        if (start % 10 == 0) {
            delay_ms(1);
        }
    }
}
// i最大速度 j最小速度
void speed_down(int high, int low)
{
    // for (; high > low; high--) {
    //     if (speed < 50) {
    //         KP = 0.0046;  // 0.005
    //         KD = 0.0027; // 0.05
    //     } else if (speed >= 50 && speed < 70) {
    //         KP = 0.0044; // 0.025
    //         KD = 0.006;  // 0.012
    //     } else if (speed >= 70 && speed <= 85) {
    //         KP = 0.0042; // 0.02
    //         KD = 0.01;   // 0.02
    //     } else if (speed >= 86 && speed < 100) {
    //         KP = 0.0035; // 0.012
    //         KD = 0.015;  // 0.3
    //     } else if (speed >= 100 && speed < 140) {
    //         KP = 0.0018;
    //         KD = 0.025;
    //     } else if (speed >= 140 && speed < 160) {
    //         KP = 0.0025;
    //         KD = 0.025;
    //     } else if (speed >= 160 && speed <= 180) {
    //         KP = 0.0022;
    //         KD = 0.03;
    //     } else if (speed > 180 && speed <= 200) {
    //         KP = 0.0018;
    //         KD = 0.015;
    //     } else if (speed > 200 && speed <= 220) {
    //         KP = 0.001;
    //         KD = 0.04;
    //     }
    for (; high > low; high--) {
        if (speed < 50) {
            KP = 0.00415; // 0.005
            KD = 0.0027;  // 0.05
        } else if (speed >= 50 && speed < 70) {
            KP = 0.00405; // 0.025
            KD = 0.006;   // 0.012
        } else if (speed >= 70 && speed <= 85) {
            KP = 0.0039; // 0.02
            KD = 0.01;   // 0.02
        } else if (speed >= 86 && speed < 100) {
            KP = 0.0033; // 0.012
            KD = 0.015;  // 0.3
        } else if (speed >= 100 && speed < 140) {
            KP = 0.0016;
            KD = 0.025;
        } else if (speed >= 140 && speed < 160) {
            KP = 0.0013; // 0.0025
            KD = 0.025;
        } else if (speed >= 160 && speed <= 180) {
            KP = 0.0012; // 0.0022
            KD = 0.03;
        } else if (speed > 180 && speed <= 200) {
            KP = 0.0016;
            KD = 0.013; // 0.0015
        } else if (speed > 200 && speed <= 220) {
            KP = 0.0008;
            KD = 0.03; // 0.04
        }
        speed = high;
        get_huidu_va();
        if (high % 5 == 0) Trace();
        delay_ms(1);
    }
}
/**
 * @brief 加减速
 *
 * @param speed_max 最大速度
 * @param speed_min 最小速度
 * @param max_time 最大速度时间
 * @param time_up 上升时间
 * @param time_down 下降时间
 */
void so_so(int speed_max, int speed_min, int max_time, int time_up, int time_down)
{
    int v_up   = speed_min;
    int v_down = speed_max;
    do {
        v_up++;
        Reset(time_up, v_up);
    } while (v_up < speed_max);
    Reset(max_time, speed_max);
    do {
        v_down--;
        Reset(time_down, v_down);
    } while (v_down > speed_min);
}
/**
 * @brief 辅助加减速
 *
 * @param speed_max 最大速度
 * @param speed_min 最小速度
 * @param max_time 最大速度时间
 * @param time_up 上升时间
 * @param time_down 下降时间
 */
void so_so_aid(int speed_max, int speed_min, int max_time, int time_up, int time_down)
{
    int v_up   = speed_min;
    int v_down = speed_max;
    do {
        v_up++;
        Reset_aid(time_up, v_up);
    } while (v_up < speed_max);
    Reset(max_time, speed_max);
    do {
        v_down--;
        Reset_aid(time_down, v_down);
    } while (v_down > speed_min);
}
/**
 * @brief 桥巡线函数
 * @param mode 模式
 */
void bridge_Trace(uint8_t mode)
{
    if (mode == 1) {
        Gray_sum = Huidu_va(0) * (1) + Huidu_va(1) * (2) + Huidu_va(2) * (3) + Huidu_va(3) * (4) + Huidu_va(4) * (5) + Huidu_va(5) * (6) +
                   Huidu_va(11) * (-1) + Huidu_va(10) * (-2) + Huidu_va(9) * (-3) + Huidu_va(8) * (-4) + Huidu_va(7) * (-5) + Huidu_va(6) * (-6);
        error = Gray_sum * 1.0 * (3000.0 / sum * 1.0);
    } else if (mode == 2) {
        Gray_sum = Huidu_va(0) * (2) + Huidu_va(1) * (2) + Huidu_va(2) * (2) + Huidu_va(3) * (2) + Huidu_va(4) * (3) + Huidu_va(5) * (3) +
                   Huidu_va(11) * (-2) + Huidu_va(10) * (-2) + Huidu_va(9) * (-2) + Huidu_va(8) * (-2) + Huidu_va(7) * (-3) + Huidu_va(6) * (-3);
        error = Gray_sum * 1.0 * (3000.0 / sum * 1.0);
    }

    motorSpeed = KP * error + KD * (error - lastError);
    lastError  = error;

    mSpeed_right = speed + motorSpeed; // 右轮速度
    mSpeed_left  = speed - motorSpeed; // 左轮速度
    lastm1Speed  = mSpeed_right;
    lastm2Speed  = mSpeed_left;
    // 限幅
    if (mSpeed_right < 0) {
        mSpeed_right = 0;
    } else if (mSpeed_right > (1.5 * speed)) {
        mSpeed_right = 1.5 * speed;
    }

    if (mSpeed_left < 0) {
        mSpeed_left = 0;
    } else if (mSpeed_left > (1.5 * speed)) {
        mSpeed_left = 1.5 * speed;
    }
    set_pwm(1, mSpeed_right, speed);
    set_pwm(2, mSpeed_left, speed);
}
/**
 * @brief 桥上巡线
 *
 * @param N 速度
 * @param mode 模式,不同权值
 */
void bridge_PD(int N, uint8_t mode)
{
    get_huidu_va();
    speed = N;
    if (mode == 1) {
        KP = 0.015;
        KD = 0.008;
    } else if (mode == 2) {
        KP = 0.009;
        KD = 0.2;
    }
    bridge_Trace(mode);
}
/**
 * @brief 梯形加减速
 *
 * @param speed_max 最大速度
 * @param speed_min 最小速度
 * @param max_time 最大速度时间
 */
void speed_UPDOWN(int speed_max, int speed_min, int max_time)
{
    speed_up(speed_min, speed_max);
    Reset(max_time, speed_max);
    speed_down(speed_max, speed_min);
}